#include "uni_motor.h"

#define BORD_RATE 115200//设定串口波特率为115200
UNI_MOTOR motor1(1, true);//定义编码电机1
UNI_MOTOR motor2(2, true);//定义编码电机2
UNI_MOTOR motor3(3, true);//定义编码电机3
UNI_MOTOR motor4(4, true);//定义编码电机4

int motor_loop_delay = 10;
float target_position = 0.2; //单位是米

void setup() {
  delay(500);
  Serial.begin(BORD_RATE);
  Serial.println("begin");
  motor1.init();
  motor2.init();
  motor3.init();
  motor4.init();
  debug_motor1(); //编码电机1，2，3，4同时到达位置1，2，3，4动作测试
}

void loop() {
  motor1.update(); //时刻保持编码电机1的位置
  motor2.update(); //时刻保持编码电机2的位置
  motor3.update(); //时刻保持编码电机3的位置
  motor4.update(); //时刻保持编码电机4的位置
  delay(10);
}

//编码电机1，2，3，4同时到达位置1，2，3，4动作测试
void debug_motor1(){
  Serial.println("-----------------position1------------------");
  motor1.set_pos(target_position);  //设定电机1需要到达的位置1（即编码电机正转target_position厘米）
  motor2.set_pos(-target_position); //设定电机2需要到达的位置1（即编码电机反转target_position厘米）
  motor3.set_pos(target_position);  //设定电机3需要到达的位置1（即编码电机正转target_position厘米）
  motor4.set_pos(-target_position); //设定电机4需要到达的位置1（即编码电机反转target_position厘米）
  while( (motor1.pos_flag) || motor2.pos_flag || motor3.pos_flag || motor4.pos_flag){
    motor1.pos_loop();
    motor2.pos_loop();
    motor3.pos_loop();
    motor4.pos_loop();
    delay(motor_loop_delay);
  }

  Serial.println("-----------------position2------------------");
  motor1.set_pos(-target_position); //设定电机1需要到达的位置2（即编码电机反转target_position厘米）
  motor2.set_pos(target_position);  //设定电机2需要到达的位置2（即编码电机正转target_position厘米）
  motor3.set_pos(-target_position); //设定电机3需要到达的位置2（即编码电机反转target_position厘米）
  motor4.set_pos(target_position);  //设定电机4需要到达的位置2（即编码电机正转target_position厘米）
  while( (motor1.pos_flag) || motor2.pos_flag || motor3.pos_flag || motor4.pos_flag){
    motor1.pos_loop();
    motor2.pos_loop();
    motor3.pos_loop();
    motor4.pos_loop();
    delay(motor_loop_delay);
  }


  Serial.println("-----------------position3------------------");
  motor1.set_pos(target_position*3);  //设定电机1需要到达的位置3（即编码电机正转target_position*3厘米）
  motor2.set_pos(-target_position*3); //设定电机2需要到达的位置3（即编码电机反转target_position*3厘米）
  motor3.set_pos(target_position*3);  //设定电机3需要到达的位置3（即编码电机正转target_position*3厘米）
  motor4.set_pos(-target_position*3); //设定电机4需要到达的位置3（即编码电机反转target_position*3厘米）
  while( (motor1.pos_flag) || motor2.pos_flag || motor3.pos_flag || motor4.pos_flag){
    motor1.pos_loop();
    motor2.pos_loop();
    motor3.pos_loop();
    motor4.pos_loop();
    delay(motor_loop_delay);
  }

  Serial.println("-----------------position4------------------");
  motor1.set_pos(-target_position*3); //设定电机1需要到达的位置4（即编码电机反转target_position*3厘米）
  motor2.set_pos(target_position*3);  //设定电机2需要到达的位置4（即编码电机正转target_position*3厘米）
  motor3.set_pos(-target_position*3); //设定电机3需要到达的位置4（即编码电机反转target_position*3厘米）
  motor4.set_pos(target_position*3);  //设定电机4需要到达的位置4（即编码电机正转target_position*3厘米）
  while( (motor1.pos_flag) || motor2.pos_flag || motor3.pos_flag || motor4.pos_flag){
    motor1.pos_loop();
    motor2.pos_loop();
    motor3.pos_loop();
    motor4.pos_loop();
    delay(motor_loop_delay);
  }
}
